Hybrid Contact Preintegration for Visual-Inertial-Contact State Estimation within Factor Graphs

The factor graph framework is a convenient modeling technique for robotic state estimation and sensor fusion where states are represented as nodes and measurements are modeled as factors. In designing a sensor fusion framework using factor graphs for legged robots, one often has access to visual, inertial, encoders, and contact sensors. While visualinertial odometry has been studied extensively in this framework, the addition of a preintegrated contact factor for legged robots has been proposed recently. In this work, to cope with the problem of switching contact frames which was not addressed previously, we propose a hybrid contact preintegration that does not require the addition of frequently broken contact factors into the estimation factor graph. This paper presents a novel method for preintegrating contact information though an arbitrary number of contact switches. The proposed hybrid modeling approach reduces the number of required variables in the nonlinear optimization problem by only requiring new states to be added alongside camera or selected keyframes. This method is evaluated using real experimental data collected from a Cassie-series robot where the trajectory of the robot produced by a motion capture system is used as a proxy for ground truth data. The evaluation shows that inclusion of the proposed preintegrated hybrid contact factor alongside visualinertial navigation systems improves robustness to vision failure as well as the estimation accuracy for legged robots while its generalization makes it more accessible for legged platforms.